package com.rockwell.maze800.test;

import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Deque;

import net.wimpi.modbus.ModbusException;
import junit.framework.Assert;
import android.test.AndroidTestCase;
import android.widget.TextView;

import com.rockwell.maze800.R;
import com.rockwell.maze800.RobotActivity;
import com.rockwell.maze800.model.DistanceBandwidthAlgorithm;
import com.rockwell.maze800.model.IMazeSolver;
import com.rockwell.maze800.model.IRobotCommander;
import com.rockwell.maze800.model.IRobotLogger;
import com.rockwell.maze800.model.IRobotPositionDetector;
import com.rockwell.maze800.model.IRobotSensors;
import com.rockwell.maze800.model.LeftFirstMazeSolver;
import com.rockwell.maze800.model.Maze;
import com.rockwell.maze800.model.Robot;
import com.rockwell.maze800.model.RobotAction;
import com.rockwell.maze800.model.RobotCommand;
import com.rockwell.maze800.model.RobotPosition;
import com.rockwell.maze800.model.RobotPositionDetector;
import com.rockwell.maze800.model.TreasureDetector;

public class LeftFirstMazeSolverTester extends AndroidTestCase {

	ArrayList<String> traceMessage = new ArrayList<String>();

	private IRobotLogger m_logger = new IRobotLogger() {

		@Override
		public void log(final String msg) {
			traceMessage.add(msg);
		}

		@Override
		public void log(final Exception ex) {
			StackTraceElement[] trace = ex.getStackTrace();
			traceMessage.add(trace.toString());
		}

		@Override
		public void onRobotPaused() {
		}

		@Override
		public void onRobotResumed() {
		}

		@Override
		public void onRobotIdle() {
		}
    };
    
    class DummyData {
		boolean[] treasurePresent;
		boolean[] barrierPresent;
		boolean[] armUp;
		double[][] positionData;
		double[] treasurePositionData;
		double[] compassData;
	}

	@Override
	public void setUp() {

	}

	@Override
	public void tearDown() {

	}

	public void testJog() {
		/*
		 * 1. Lane Center
		 * 		Send Jog Done @ performJog, 
		 *		Wait @ performJog
		 * 2. Too Left
		 * 		** Adjust**
		 * 		AmountToTurn[-5], Adjust[-5] @ performAdjust, Stop Done @ performAdjust, Adjust Done @ performAdjust, Jog Done @ performAdjust, 
		 * 		Send Jog Done @ performJog
		 * 3. Adjust to Center
		 * 		Wait @ performJog
		 * 4. Lane Center
		 * 		**Adjust back**
		 * 		AmountToTurn[5], Adjust[0] @ performAdjust, Stop Done @ performAdjust, Adjust Done @ performAdjust, Jog Done @ performAdjust, 
		 *		Send Jog Done @ performJog
		 * 5. Too Right
		 * 		**Adjust**
		 * 		AmountToTurn[5], Adjust[5] @ performAdjust, Stop Done @ performAdjust, Adjust Done @ performAdjust, Jog Done @ performAdjust, 
		 *		Send Jog Done @ performJog
		 * 6. Adjust to Right
		 * 		Wait @ performJog
		 * 7. Lane Center
		 * 		**Adjust back**
		 * 		AmountToTurn[-5], Adjust[0] @ performAdjust, Stop Done @ performAdjust, Adjust Done @ performAdjust, Jog Done @ performAdjust, 
		 *		Send Jog Done @ performJog
		 * 8. Lane Center
		 * 		Wait @ performJog	
		 */
		DummyData jogData = new DummyData()
		{{
			treasurePresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
					false, //test case 3
					false, //test case 4
					false, //test case 5
					false, //test case 6
					false, //test case 7
					false, //test case 8
			};
			
			barrierPresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
					false, //test case 3
					false, //test case 4
					false, //test case 5
					false, //test case 6
					false, //test case 7
					false, //test case 8
			};
			
			armUp = new boolean[] { 
					true, //test case 1
					true, //test case 2
					true, //test case 3
					true, //test case 4
					true, //test case 5
					true, //test case 6
					true, //test case 7
					true, //test case 8
			};
			
			positionData = new double[][] { 
					{0, RobotPositionDetector.LeftLaneCenterDistance, RobotPositionDetector.LeftLaneCenterDistance}, //test case 1
					{0, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 2
					{0, RobotPositionDetector.LeftCloseToSideWallDistance, RobotPositionDetector.LeftLaneCenterDistance + 1}, //test case 3
					{0, RobotPositionDetector.LeftLaneCenterDistance, RobotPositionDetector.LeftLaneCenterDistance}, //test case 4
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.LeftTooCloseToSideWallDistance}, //test case 5
					{0, RobotPositionDetector.LeftLaneCenterDistance + 1, RobotPositionDetector.LeftCloseToSideWallDistance}, //test case 6
					{0, RobotPositionDetector.LeftLaneCenterDistance, RobotPositionDetector.LeftLaneCenterDistance}, //test case 7
					{0, RobotPositionDetector.LeftLaneCenterDistance, RobotPositionDetector.LeftLaneCenterDistance}, //test case 8
			};
		}};

		String[] expectedMessage = { 	
				"Send Jog Done @ performJog",
				"Wait @ performJog",
				"AmountToTurn[-5], Adjust[-5] @ performAdjust",
				"Stop Done @ performAdjust",
				"Adjust Done @ performAdjust",
				"Jog Done @ performAdjust",
				"Send Jog Done @ performJog",
				"Wait @ performJog",
				"AmountToTurn[5], Adjust[0] @ performAdjust",
				"Stop Done @ performAdjust",
				"Adjust Done @ performAdjust",
				"Jog Done @ performAdjust",
				"Send Jog Done @ performJog",
				"AmountToTurn[5], Adjust[5] @ performAdjust",
				"Stop Done @ performAdjust",
				"Adjust Done @ performAdjust",
				"Jog Done @ performAdjust",
				"Send Jog Done @ performJog",
				"Wait @ performJog",
				"AmountToTurn[-5], Adjust[0] @ performAdjust",
				"Stop Done @ performAdjust",
				"Adjust Done @ performAdjust",
				"Jog Done @ performAdjust",
				"Send Jog Done @ performJog",
				"Wait @ performJog"
				};
		
		RobotAction[] expectedActions = {
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog
		};
		
		DummyLRFRobotSensors sensors = new DummyLRFRobotSensors(jogData.positionData, jogData.armUp, null);
		DummyRobotCommander commander = new DummyRobotCommander();
		LeftFirstMazeSolver solver = new LeftFirstMazeSolver();
		DummyTreasureDetector treasureDetector = new DummyTreasureDetector(jogData.treasurePositionData, jogData.treasurePresent);
		DummyBarrierDetector barrierDetector = new DummyBarrierDetector(jogData.barrierPresent);

		Maze maze = new Maze(30, 5);

		Robot robot = new Robot(15, 15, sensors, commander, maze, solver, treasureDetector, barrierDetector);
		robot.setWidth(15);
		robot.setLength(15);

		robot.setLogger(m_logger, 4);
		robot.setStepMode(false);
		
		traceMessage.clear();
		robot.testJog(4);
		
		verifyResult(commander, jogData, sensors, treasureDetector, expectedMessage, expectedActions);
	}
	
	public void testPick() {
		/*
		 * 1. Lane Center
		 * 		Send Jog Done @ performJog, 
		 *		Wait @ performJog
		 * 2. Too Left
		 * 		** Adjust**
		 * 		AmountToTurn[-5], Adjust[-5] @ performAdjust, Stop Done @ performAdjust, Adjust Done @ performAdjust, Jog Done @ performAdjust, 
		 * 		Send Jog Done @ performJog
		 * 3. Treasure detected
		 * 	
		 */
		DummyData pickData = new DummyData()
		{{
			treasurePresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
					true,  //test case 3 in performJog.DETECT_TREASURE
					true,  //test case 3 in performPick.STOP_ROBOT
					true,  //test case 3 in performPick.CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK
					true,  //test case 3 in performPick.CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK
					true,  //test case 3 in performPick.CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK
					true,  //test case 3 in performPick.CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK
					false, //test case 3 in performPick.CHECK_FOR_TREASURE_ABSENCE_BEFORE_PICK
					false,  //test case 3 in performPick.VERIFY_TREASURE_PICKED
			};
			
			barrierPresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
					false, //test case 3 in performJog.DETECT_BARRIER
			};
			
			treasurePositionData = new double[] {
					7, //test case 3 in performPick.GET_TREASURE_POSITION
					6, //test case 3 in performPick.GET_TREASURE_POSITION
					5, //test case 3 in performPick.GET_TREASURE_POSITION
					0, //test case 3 in performPick.GET_TREASURE_POSITION
			};
			
			compassData = new double[] {
					0, //test case 3 in performPick.STOP_ROBOT
					5, //test case 3 in performPick.CHECK_DRIFT_FROM_INITIAL_ORIENTATION
					0, //test case 3 in performPick.CHECK_DRIFT_FROM_INITIAL_ORIENTATION
					2, //test case 3 in performPick.CAPTURE_ORIENTATION_BEFORE_TURN
					170, //test case 3 in performPick.CHECK_ORIENTATION
					182, //test case 3 in performPick.CHECK_ORIENTATION
			};
			
			armUp = new boolean[] { 
					true, //test case 1
					true, //test case 2
					true, //test case 3 in performPick.ENSURE_ARM_UP_AFTER_PICK
					true, //test case 3 in performTurnBack.ENSURE_ARM_UP
			};
			
			positionData = new double[][] { 
					{0, RobotPositionDetector.LeftLaneCenterDistance, RobotPositionDetector.LeftLaneCenterDistance}, //test case 1
					{0, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 2
					{1, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performTurnBack.CHECK_IF_JUCTION_REACHED
					{0, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performTurnBack.CHECK_IF_JUCTION_REACHED
					{0, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performTurnBack.CHECK_IF_JUCTION_REACHED
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performTurnBack.CHECK_IF_JUCTION_REACHED
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performTurnBack.CHECK_IF_OUT_OF_JUCNTION
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performTurnBack.CHECK_IF_OUT_OF_JUCNTION
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performTurnBack.CHECK_IF_OUT_OF_JUCNTION
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.LeftTooCloseToSideWallDistance}, //test case 3 in performTurnBack.CHECK_IF_OUT_OF_JUCNTION
			};
		}};

		String[] expectedMessage = { 	
				"Send Jog Done @ performJog", 
				"Wait @ performJog", 
				"AmountToTurn[-5], Adjust[-5] @ performAdjust", 
				"Stop Done @ performAdjust", 
				"Adjust Done @ performAdjust", 
				"Jog Done @ performAdjust", 
				"Send Jog Done @ performJog", 
				"Treasure Detected @ performJog", 
				"Stop Done @ performAdjustBack", 
				"Adjust Back[5] @ performAdjustBack",
				"Adjust Back Done @ continueMission",
				"Start Pick @ performPick", 
				"Stop Done @ performPick", 
				"Turn[2] to Horizontal Pick Center Done @ performPick", 
				"Turn[2] to Horizontal Pick Center Done @ performPick", 
				"Turn[2] to Horizontal Pick Center Done @ performPick", 
				"Locate Treasure at Horizontal Pick Center Done @ performPick", 
				"Arm Down Done @ performPick", 
				"Move to Block Treasure Done @ performPick", 
				"Pick Done @ performPick", 
				"Move Backward Done @ performPick", 
				"Ensure Arm Up Done @ performPick", 
				"Compensation Angle[-5] after Turn @ performPick", 
				"Compensation Angle[0] after Turn @ performPick", 
				"Start Turn Back @ performTurnBack", 
				"Stop Done @ performTurnBack", 
				"Ensure Arm Up Done @ performTurnBack", 
				"Junction Reached During Reverse @ performTurnBack", 
				"Reverse Into Junction Done @ performTurnBack", 
				"Orientation[2] before U Turn @ performTurnBack", 
				"U Turn Done @ performTurnBack", 
				"Compensation Angle[12] after U Turn @ performTurnBack", 
				"Compensation Angle[0] after U Turn @ performTurnBack", 
				"Move Out Of Junction Done @ performTurnBack", 
				"Post Move Out Of Junction Done @ performTurnBack", 
				"U Turn Done @ performPick",
				"Pick Treasure Done @ continueMission",
				};
		
		RobotAction[] expectedActions = {
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.TurnDegree, 
				RobotAction.TurnDegree, 
				RobotAction.ArmDown, 
				RobotAction.Move, 
				RobotAction.Move, 
				RobotAction.Move, 
				RobotAction.Move, 
				RobotAction.PickTreasure, 
				RobotAction.MoveBackward, 
				RobotAction.TurnDegree, 
				RobotAction.Stop, 
				RobotAction.MoveBackward, 
				RobotAction.MoveBackward, 
				RobotAction.MoveBackward, 
				RobotAction.MoveBackward, 
				RobotAction.TurnBack, 
				RobotAction.TurnDegree, 
				RobotAction.MoveBackward, 
				RobotAction.MoveBackward, 
				RobotAction.MoveBackward, 
				RobotAction.MoveBackward,
		};
		
		DummyLRFRobotSensors sensors = new DummyLRFRobotSensors(pickData.positionData, pickData.armUp, pickData.compassData);
		DummyRobotCommander commander = new DummyRobotCommander();
		LeftFirstMazeSolver solver = new LeftFirstMazeSolver();
		DummyTreasureDetector treasureDetector = new DummyTreasureDetector(pickData.treasurePositionData, pickData.treasurePresent);
		DummyBarrierDetector barrierDetector = new DummyBarrierDetector(pickData.barrierPresent);
		
		Maze maze = new Maze(30, 5);

		Robot robot = new Robot(15, 15, sensors, commander, maze, solver, treasureDetector, barrierDetector);
		robot.setWidth(15);
		robot.setLength(15);

		robot.setLogger(m_logger, 4);
		robot.setStepMode(false);
		
		traceMessage.clear();
		
		Deque<Robot.RunEnum> actions = new ArrayDeque<Robot.RunEnum>(4);
		actions.add(Robot.RunEnum.PERFORM_JOG);
		actions.add(Robot.RunEnum.PERFORM_ADJUST_BACK);
		actions.add(Robot.RunEnum.PERFORM_PICK);
		actions.add(Robot.RunEnum.PERFORM_JOG);
		robot.testContinueMission(actions);
		
		verifyResult(commander, pickData, sensors, treasureDetector, expectedMessage, expectedActions);
	}
	
	public void testTurn() {
		/*
		 * 1. Lane Center
		 * 		Send Jog Done @ performJog, 
		 *		Wait @ performJog
		 * 2. Too Left
		 * 		** Adjust**
		 * 		AmountToTurn[-5], Adjust[-5] @ performAdjust, Stop Done @ performAdjust, Adjust Done @ performAdjust, Jog Done @ performAdjust, 
		 * 		Send Jog Done @ performJog
		 * 3. Turn Left only
		 * 4. Turn Left Straight
		 * 5. Go Straight
		 * 6. Turn Right Only
		 * 
		 */
		DummyData turnData = new DummyData()
		{{
			treasurePresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
					false, //test case 3 in performJog.DETECT_TREASURE
					false, //test case 4 in performJog.DETECT_TREASURE
					false, //test case 5 in performJog.DETECT_TREASURE
					false, //test case 6 in performJog.DETECT_TREASURE
			};
			
			barrierPresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
					false, //test case 3 in performJog.DETECT_BARRIER
					false, //test case 4 in performJog.DETECT_BARRIER
					false, //test case 5 in performJog.DETECT_BARRIER
					false, //test case 6 in performJog.DETECT_BARRIER
			};
			
			compassData = new double[] {
					0, //test case 3 in performTurn.CAPTURE_ORIENTATION_BEFORE_TURN
					95, //test case 3 in performTurn.CHECK_ORIENTATION
					90, //test case 3 in performTurn.CHECK_ORIENTATION
					0, //test case 4 in performTurn.CAPTURE_ORIENTATION_BEFORE_TURN
					85, //test case 4 in performTurn.CHECK_ORIENTATION
					90, //test case 4 in performTurn.CHECK_ORIENTATION
					0, //test case 6 in performTurn.CAPTURE_ORIENTATION_BEFORE_TURN
					-95, //test case 6 in performTurn.CHECK_ORIENTATION
					-90, //test case 6 in performTurn.CHECK_ORIENTATION
			};
			
			armUp = new boolean[] { 
					true, //test case 1
					true, //test case 2
					true, //test case 3 in performJog.ENSURE_ARM_UP
					true, //test case 3 in performDetectFrontWall.ENSURE_ARM_UP
					true, //test case 3 in performTurn.ENSURE_ARM_UP
					true, //test case 4 in performJog.ENSURE_ARM_UP
					true, //test case 4 in performDetectFrontWall.ENSURE_ARM_UP
					true, //test case 4 in performTurn.ENSURE_ARM_UP
					true, //test case 5 in performJog.ENSURE_ARM_UP
					true, //test case 5 in performDetectFrontWall.ENSURE_ARM_UP
					true, //test case 6 in performJog.ENSURE_ARM_UP
					true, //test case 6 in performDetectFrontWall.ENSURE_ARM_UP
					true, //test case 6 in performTurn.ENSURE_ARM_UP
			};
			
			positionData = new double[][] { 
					/* 1 - 5*/
					{0, RobotPositionDetector.LeftLaneCenterDistance, RobotPositionDetector.LeftLaneCenterDistance}, //test case 1
					{0, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 2
					//test case 3
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performJog.UPDATE_NEXT_ACTION
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{1, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{1, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performTurn.CHECK_TO_ENSURE_FRONT_SENSOR_OFF
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 3 in performTurn.CHECK_TO_ENSURE_FRONT_SENSOR_OFF
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftTooCloseToSideWallDistance}, //test case 3 in performTurn.CHECK_IF_OUT_OF_JUCNTION
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftTooCloseToSideWallDistance}, //test case 3 in performTurn.CHECK_IF_OUT_OF_JUCNTION
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.LeftTooCloseToSideWallDistance}, //test case 3 in performTurn.CHECK_IF_OUT_OF_JUCNTION
					//test case 4
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performJog.UPDATE_NEXT_ACTION
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.SideWallDistanceThreshold + 2, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.LeftTooCloseToSideWallDistance}, //test case 4 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.LeftTooCloseToSideWallDistance}, //test case 4 in performTurn.CHECK_IF_OUT_OF_JUCNTION
					//test case 5
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 5 in performJog.UPDATE_NEXT_ACTION
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 5 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 5 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 5 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 5 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 5 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.LeftTooCloseToSideWallDistance}, //test case 5 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.LeftTooCloseToSideWallDistance}, //test case 5 in performDetectFrontWall.CONTINUE_JOGGING
					//test case 6
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performJog.UPDATE_NEXT_ACTION
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{1, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performDetectFrontWall.INDEX_TO_DETECT_FRONT_WALL
					{1, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performTurn.CHECK_TO_ENSURE_FRONT_SENSOR_OFF
					{0, RobotPositionDetector.LeftLaneCenterDistance + 2, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performTurn.CHECK_TO_ENSURE_FRONT_SENSOR_OFF
					{0, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performTurn.CHECK_IF_OUT_OF_JUCNTION
					{0, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.SideWallDistanceThreshold + 2}, //test case 6 in performTurn.CHECK_IF_OUT_OF_JUCNTION
					{0, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 6 in performTurn.CHECK_IF_OUT_OF_JUCNTION
			};
		}};

		String[] expectedMessage = { 
				"Send Jog Done @ performJog", 
				"Wait @ performJog", 
				"AmountToTurn[-5], Adjust[-5] @ performAdjust", 
				"Stop Done @ performAdjust", 
				"Adjust Done @ performAdjust", 
				"Jog Done @ performAdjust", 
				"Send Jog Done @ performJog", 
				"Need Detect Front Wall. Location[TLeftStraight] @ performDetectFrontWall", 
				"Stop Done @ performDetectFrontWall", 
				"Stop Done @ performAdjustBack", 
				"Adjust Back[5] @ performAdjustBack", 
				"Adjust Back Done @ performDetectFrontWall", 
				"Ensure Arm Up Done @ performDetectFrontWall", 
				"Amount to move forward[400] @ performDetectFrontWall", 
				"Move Forward Done. Remaining[355], Front Wall[YES] @ performDetectFrontWall", 
				"Turn Left @ determineNextAction", 
				"Update Next Action Done. Location[LeftTurnOnly] @ performDetectFrontWall", 
				"Start Turn[90] @ performTurn", 
				"Stop Done @ performTurn", 
				"Ensure Arm Up Done @ performTurn", 
				"Ensure Front Sensor OFF Done @ performTurn", 
				"Orientation[0] before Turn @ performTurn", 
				"Turn Done @ performTurn", 
				"Compensation Angle[-5] after Turn @ performTurn", 
				"Compensation Angle[0] after Turn @ performTurn", 
				"Move Out Of Junction Done @ performTurn", 
				"Post Move Out Of Junction Done @ performTurn", 
				"Turn Left Done @ continueMission", //< end of test case 3
				"Send Jog Done @ performJog", 
				"Need Detect Front Wall. Location[TLeftStraight] @ performDetectFrontWall", 
				"Stop Done @ performDetectFrontWall", 
				"Adjust Back Done @ performDetectFrontWall", 
				"Ensure Arm Up Done @ performDetectFrontWall", 
				"Amount to move forward[400] @ performDetectFrontWall", 
				"Move Forward Done. Remaining[325], Front Wall[NO] @ performDetectFrontWall", 
				"Turn Left @ determineNextAction", 
				"Update Next Action Done. Location[TLeftStraight] @ performDetectFrontWall", 
				"Start Turn[90] @ performTurn", 
				"Stop Done @ performTurn", 
				"Ensure Arm Up Done @ performTurn", 
				"Move Without Front Wall Done @ performTurn", 
				"Orientation[0] before Turn @ performTurn", 
				"Turn Done @ performTurn", 
				"Compensation Angle[5] after Turn @ performTurn", 
				"Compensation Angle[0] after Turn @ performTurn", 
				"Move Out Of Junction Done @ performTurn", 
				"Post Move Out Of Junction Done @ performTurn", 
				"Turn Left Done @ continueMission", //< end of test case 4 
				"Send Jog Done @ performJog", 
				"Need Detect Front Wall. Location[TRightStraight] @ performDetectFrontWall", 
				"Stop Done @ performDetectFrontWall", 
				"Adjust Back Done @ performDetectFrontWall", 
				"Ensure Arm Up Done @ performDetectFrontWall", 
				"Amount to move forward[400] @ performDetectFrontWall", 
				"Move Forward Done. Remaining[355], Front Wall[NO] @ performDetectFrontWall", 
				"Continue Jogging @ determineNextAction", 
				"Update Next Action Done. Location[TRightStraight] @ performDetectFrontWall", 
				"Continue Jogging Done @ performDetectFrontWall", 
				"Post Move for Continue Jogging Done @ performDetectFrontWall", //< end of test case 5
				"Send Jog Done @ performJog", 
				"Need Detect Front Wall. Location[TRightStraight] @ performDetectFrontWall", 
				"Stop Done @ performDetectFrontWall", 
				"Adjust Back Done @ performDetectFrontWall", 
				"Ensure Arm Up Done @ performDetectFrontWall", 
				"Amount to move forward[400] @ performDetectFrontWall", 
				"Move Forward Done. Remaining[355], Front Wall[YES] @ performDetectFrontWall", 
				"Turn Right @ determineNextAction", 
				"Update Next Action Done. Location[RightTurnOnly] @ performDetectFrontWall", 
				"Start Turn[-90] @ performTurn", 
				"Stop Done @ performTurn", 
				"Ensure Arm Up Done @ performTurn", 
				"Ensure Front Sensor OFF Done @ performTurn", 
				"Orientation[0] before Turn @ performTurn", 
				"Turn Done @ performTurn", 
				"Compensation Angle[5] after Turn @ performTurn", 
				"Compensation Angle[0] after Turn @ performTurn", 
				"Move Out Of Junction Done @ performTurn", 
				"Post Move Out Of Junction Done @ performTurn", 
				"Turn Right Done @ continueMission", //< End of test case 6
				};
		
		RobotAction[] expectedActions = {
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.Stop, 
				RobotAction.MoveBackward, 
				RobotAction.TurnLeft, 
				RobotAction.TurnDegree, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, //< End of test case 3
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.Stop, 
				RobotAction.MoveBackward, 
				RobotAction.TurnLeft, 
				RobotAction.TurnDegree, 
				RobotAction.MoveForward,  //< End of test case 4
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward,  //< End of test case 5
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.Stop, 
				RobotAction.MoveBackward, 
				RobotAction.TurnRight, 
				RobotAction.TurnDegree, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, 
				RobotAction.MoveForward, //< End of test case 6
		};
		
		DummyLRFRobotSensors sensors = new DummyLRFRobotSensors(turnData.positionData, turnData.armUp, turnData.compassData);
		DummyRobotCommander commander = new DummyRobotCommander();
		LeftFirstMazeSolver solver = new LeftFirstMazeSolver();
		DummyTreasureDetector treasureDetector = new DummyTreasureDetector(turnData.treasurePositionData, turnData.treasurePresent);
		DummyBarrierDetector barrierDetector = new DummyBarrierDetector(turnData.barrierPresent);
		
		Maze maze = new Maze(30, 5);

		Robot robot = new Robot(15, 15, sensors, commander, maze, solver, treasureDetector, barrierDetector);
		robot.setWidth(15);
		robot.setLength(15);

		robot.setLogger(m_logger, 4);
		robot.setStepMode(false);
		
		traceMessage.clear();
		
		Deque<Robot.RunEnum> actions = new ArrayDeque<Robot.RunEnum>(8);
		actions.add(Robot.RunEnum.PERFORM_JOG);
		actions.add(Robot.RunEnum.PERFORM_TURN_LEFT);
		actions.add(Robot.RunEnum.PERFORM_JOG);
		actions.add(Robot.RunEnum.PERFORM_TURN_LEFT);
		actions.add(Robot.RunEnum.PERFORM_JOG);
		actions.add(Robot.RunEnum.PERFORM_TURN_RIGHT);
		actions.add(Robot.RunEnum.PERFORM_JOG);
		robot.testContinueMission(actions);
		
		verifyResult(commander, turnData, sensors, treasureDetector, expectedMessage, expectedActions);
	}

	public void _testUTurn() {
		/*
		 * 1. Lane Center
		 * 		Send Jog Done @ performJog, 
		 *		Wait @ performJog
		 * 2. Too Left
		 * 		** Adjust**
		 * 		AmountToTurn[-5], Adjust[-5] @ performAdjust, Stop Done @ performAdjust, Adjust Done @ performAdjust, Jog Done @ performAdjust, 
		 * 		Send Jog Done @ performJog
		 * 3. Adjust back
		 * 4. U Turn
		 */
		DummyData turnData = new DummyData()
		{{
			treasurePresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
			};
			
			barrierPresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
			};
			
			compassData = new double[] {
			};
			
			armUp = new boolean[] { 
					true, //test case 1
					true, //test case 2
			};
			
			positionData = new double[][] { 
					/* 1 - 5*/
					{0, RobotPositionDetector.LeftLaneCenterDistance, RobotPositionDetector.LeftLaneCenterDistance}, //test case 1
					{0, RobotPositionDetector.LeftTooCloseToSideWallDistance, RobotPositionDetector.LeftLaneCenterDistance + 2}, //test case 2
			};
		}};

		String[] expectedMessage = { 	
				"Send Jog Done @ performJog", 
				"Wait @ performJog", 
				"AmountToTurn[-5], Adjust[-5] @ performAdjust", 
				"Stop Done @ performAdjust", 
				"Adjust Done @ performAdjust", 
				"Jog Done @ performAdjust", 
				"Send Jog Done @ performJog", //< End of test case 2
				};
		
		RobotAction[] expectedActions = {
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
		};
		
		DummyLRFRobotSensors sensors = new DummyLRFRobotSensors(turnData.positionData, turnData.armUp, turnData.compassData);
		DummyRobotCommander commander = new DummyRobotCommander();
		LeftFirstMazeSolver solver = new LeftFirstMazeSolver();
		DummyTreasureDetector treasureDetector = new DummyTreasureDetector(turnData.treasurePositionData, turnData.treasurePresent);
		DummyBarrierDetector barrierDetector = new DummyBarrierDetector(turnData.barrierPresent);
		
		Maze maze = new Maze(30, 5);

		Robot robot = new Robot(15, 15, sensors, commander, maze, solver, treasureDetector, barrierDetector);
		robot.setWidth(15);
		robot.setLength(15);

		robot.setLogger(m_logger, 4);
		robot.setStepMode(false);
		
		traceMessage.clear();
		
		Deque<Robot.RunEnum> actions = new ArrayDeque<Robot.RunEnum>(4);
		actions.add(Robot.RunEnum.PERFORM_JOG);
		actions.add(Robot.RunEnum.PERFORM_ADJUST_BACK);
		actions.add(Robot.RunEnum.PERFORM_TURN_BACK);
		actions.add(Robot.RunEnum.PERFORM_JOG);
		robot.testContinueMission(actions);
		
		verifyResult(commander, turnData, sensors, treasureDetector, expectedMessage, expectedActions);
	}

	public void verifyResult(
			DummyRobotCommander commander,
			DummyData dummyData, 
			DummyLRFRobotSensors sensors, 
			DummyTreasureDetector treasureDetector, 
			String[] expectedMessage,
			RobotAction[] expectedActions) {
		if (dummyData.armUp != null) {
			Assert.assertEquals(sensors.m_armUpIndex, dummyData.armUp.length);
		}
		if (dummyData.compassData != null) {
			Assert.assertEquals(sensors.m_compassIndex, dummyData.compassData.length);
		}
		if (dummyData.positionData != null) {
			Assert.assertEquals(sensors.m_frontIndex, dummyData.positionData.length);
			Assert.assertEquals(sensors.m_leftIndex, dummyData.positionData.length);
			Assert.assertEquals(sensors.m_rightIndex, dummyData.positionData.length);
		}
		if (dummyData.treasurePresent != null) {
			Assert.assertEquals(treasureDetector.m_treasurePresentIndex, dummyData.treasurePresent.length);
		}
		if (dummyData.treasurePositionData != null) {
			Assert.assertEquals(treasureDetector.m_treasurePositionIndex, dummyData.treasurePositionData.length);
		}
		
		Assert.assertEquals(expectedMessage.length, traceMessage.size());
		
		int i = 0;
		for (String msg : traceMessage) {
			Assert.assertEquals(expectedMessage[i], msg);
			i++;
		}
		
		ArrayList<RobotAction> actions = commander.getActions();
		
		i = 0;
		for (RobotAction action : actions) {
			Assert.assertEquals(expectedActions[i], action);
			i++;
		}
	}
}
